
// ================================================================================================
// -*- C++ -*-
// File: DijkstrasSearch.hpp
// Author: Guilherme R. Lampert
// Created on: 05/10/13
// Brief: Graph search using the popular Dijkstra's algorithm.
// ================================================================================================

#ifndef __DIJKSTRAS_SEARCH_HPP__
#define __DIJKSTRAS_SEARCH_HPP__

#include "IndexedPriorityQueue.hpp"

// Dijkstra's Search Algorithm:
//
// Given a graph, source and optional target, this class solves for
// single source shortest paths (without a target being specified) or
// shortest path from source to target.
// The algorithm used is a priority queue implementation of Dijkstra's.
//
template
<
	class GraphType,
	class TerminatingCondition
>
class DijkstrasSearch {

public:

	// Graph edge type:
	typedef typename GraphType::EdgeType Edge;

	// Sets up the search. Call RunSearch() to actually perform it.
	inline DijkstrasSearch(GraphType & graph) :
		myGraph(graph), sourceNode(-1), targetNode(-1) { }

	// Prepares the search algorithm for a new graph search.
	// Always call this function at least once before RunSearch().
	// Must also be called every time the graph has new nodes added to it.
	inline void SetupSearch()
	{
		const unsigned int numNodes = myGraph.NumNodes();
		if (numNodes != 0)
		{
			shortestPathTree.resize(numNodes);
			costToThisNode.resize(numNodes);
			searchFrontier.resize(numNodes);
		}
	}

	// Set target and source nodes. Should be called before performing the search,
	// since default values are -1 for both source and target.
	inline void SetSourceNode(int src) { sourceNode = src; }
	inline void SetTargetNode(int target) { targetNode = target; }

	// Get current target and source nodes:
	inline int GetTargetNode() const { return (targetNode); }
	inline int GetSourceNode() const { return (sourceNode); }

	// Fires the search. Returns true if the target node requested was found. False otherwise.
	inline bool RunSearch();

	// Returns the vector of edges that defines the SPT. If a target was given
	// in the constructor then this will be an SPT comprising of all the nodes
	// examined before the target was found, else it will contain all the nodes in the graph.
	inline const std::vector<const Edge *> & GetSPT() const { return (shortestPathTree); }

	// Returns a vector of node indexes that comprise the shortest path
	// from the source to the target. It calculates the path by working
	// backwards through the SPT from the target node.
	inline std::list<int> GetPathToTarget() const;

	// Returns the total cost to the target.
	inline double GetCostToTarget() const { return (costToThisNode[targetNode]); }

	// Returns the total cost to the given node.
	inline double GetCostToNode(unsigned int nd) const { return (costToThisNode[nd]); }

private:

	// Graph we are searching:
	GraphType & myGraph;

	// This vector contains the edges that comprise the shortest path tree -
	// a directed subtree of the graph that encapsulates the best paths from
	// every node on the SPT to the source node.
	std::vector<const Edge *> shortestPathTree;

	// This is indexed into by node index and holds the total cost of the best
	// path found so far to the given node. For example, costToThisNode[5]
	// will hold the total cost of all the edges that comprise the best path
	// to node 5, found so far in the search (if node 5 is present and has been visited).
	std::vector<double> costToThisNode;

	// This is an indexed (by node) vector of 'parent' edges leading to nodes
	// connected to the SPT but that have not been added to the SPT yet.
	std::vector<const Edge *> searchFrontier;

	// Source and target node indexes for the search.
	int sourceNode;
	int targetNode;
};

//-----------------------------------------------------------------------------

template<class GraphType, class TerminatingCondition>
bool DijkstrasSearch<GraphType, TerminatingCondition>::RunSearch()
{
	shortestPathTree.clear();
	costToThisNode.clear();
	searchFrontier.clear();
	SetupSearch();

	// create an indexed priority queue that sorts smallest to largest
	// (front to back). Note that the maximum number of elements the iPQ
	// may contain is N. This is because no node can be represented on the
	// queue more than once.
	IndexedPriorityQLow<double> pq(costToThisNode, myGraph.NumNodes());

	// put the source node on the queue
	pq.Insert(sourceNode);

	// while the queue is not empty
	while (!pq.IsEmpty())
	{
		// get lowest cost node from the queue. Don't forget, the return value
		// is a *node index*, not the node itself. This node is the node not already
		// on the SPT that is the closest to the source node
		int NextClosestNode = pq.Pop();

		// move this edge from the frontier to the shortest path tree
		shortestPathTree[NextClosestNode] = searchFrontier[NextClosestNode];

		// Done when user defined termination condition is satisfied.
		if (TerminatingCondition::IsSatisfied(myGraph, targetNode, NextClosestNode))
		{
			targetNode = NextClosestNode;
			return (true);
		}

		// now to relax the edges.
		typename GraphType::EdgeIterator EdgeItr(myGraph, NextClosestNode);

		// for each edge connected to the next closest node
		for (const Edge * pE = EdgeItr.begin(); !EdgeItr.end(); pE = EdgeItr.next())
		{
			// the total cost to the node this edge points to is the cost to the
			// current node plus the cost of the edge connecting them.
			double NewCost = costToThisNode[NextClosestNode] + pE->Cost(myGraph.GetNode(pE->From()), myGraph.GetNode(pE->To()));

			// if this edge has never been on the frontier make a note of the cost
			// to get to the node it points to, then add the edge to the frontier
			// and the destination node to the PQ.
			if (searchFrontier[pE->To()] == 0)
			{
				costToThisNode[pE->To()] = NewCost;
				pq.Insert(pE->To());
				searchFrontier[pE->To()] = pE;
			}
			else if ((NewCost < costToThisNode[pE->To()]) && (shortestPathTree[pE->To()] == 0))
			{
				// else test to see if the cost to reach the destination node via the
				// current node is cheaper than the cheapest cost found so far. If
				// this path is cheaper, we assign the new cost to the destination
				// node, update its entry in the PQ to reflect the change and add the
				// edge to the frontier
				costToThisNode[pE->To()] = NewCost;

				// because the cost is less than it was previously, the PQ must be
				// re-sorted to account for this.
				pq.ChangePriority(pE->To());

				searchFrontier[pE->To()] = pE;
			}
		}
	}

	// If we get here there is no possible route to the target node.
	return (false);
}

//-----------------------------------------------------------------------------

template<class GraphType, class TerminatingCondition>
std::list<int> DijkstrasSearch<GraphType, TerminatingCondition>::GetPathToTarget() const
{
	std::list<int> path;

	// Just return an empty path if no target or no path found:
	if (targetNode < 0)
	{
		return (path);
	}

	int nd = targetNode;
	path.push_front(nd);

	while ((nd != sourceNode) && (shortestPathTree[nd] != 0))
	{
		nd = shortestPathTree[nd]->From();
		path.push_front(nd);
	}

	return (path);
}

#endif // __DIJKSTRAS_SEARCH_HPP__
